#include "ros/ros.h"
#include <geometry_msgs/Point.h>

void formationTargetCallback1(const geometry_msgs::Point::ConstPtr& msg)
{
    ROS_INFO("Received tb3_1 target: x = %f, y = %f", msg->x, msg->y);
}

void formationTargetCallback2(const geometry_msgs::Point::ConstPtr& msg)
{
    ROS_INFO("Received tb3_2 target: x = %f, y = %f", msg->x, msg->y);
}

int main(int argc, char** argv)
{
    // Initialize ROS node
    ros::init(argc, argv, "formation_target_listener");

    ros::NodeHandle nh;

    // Subscribe to the target position topics
    ros::Subscriber sub_tb3_1 = nh.subscribe("tb3_1/formation_target", 10, formationTargetCallback1);
    ros::Subscriber sub_tb3_2 = nh.subscribe("tb3_2/formation_target", 10, formationTargetCallback2);

    ros::spin(); // Keep the node running and listening to the topics

    return 0;
}
